/*
  Copyright (c) 2008 Florent D'halluin , Sylvain Calinon, 
  LASA Lab, EPFL, CH-1015 Lausanne, Switzerland, 
  http://www.calinon.ch, http://lasa.epfl.ch

  The program is free for non-commercial academic use. 
*/

#include "MathLib.h"
#include "Quaternion.h"
#include <iostream>
#include <fstream>

/**
 * \defgroup IK  Kinematics 
 * \brief direct and inverse kinematics computation 
 */

/**
 * class representing one segment of kinematics chain 
 * \ingroup IK
 */

class Segment
{
 public :
  short axe; /* index of the axis of rotation */
  Vector3 seg;
  float max_joint_angle;
  float min_joint_angle;
  Segment()
    {
      axe = 1;
    };
  Segment(short axe,Vector3 seg)
    {
      this->max_joint_angle=0.;
      this->min_joint_angle =0.;
      this->axe = axe;
      this->seg = seg;
    };
  /**
   * get the Homgeneous Matrix corresponding to the 
   * translation bring by the segment and the rotation 
   * bring by the joint. (at given angle) 
   */
  Matrix4 get_matrix(float angle);
  
  /**
   * get the derivate Homogeneous Matrix 
   */
  Matrix4 get_diff_mat(float angle);
  
  /**
   * get a Quaternion corresponding to rotation of segment 
   */
  Quaternion get_quaternion(float angle);
  Quaternion get_quaternion_speed(float angle);

  /**
   *  return the gradient for WLN limit avoidance
   *  
   *          (max - min)^2 * (2*a - max - min)
   *  dH/da    --------------------------------
   *            4*(max - a)^2 * (a - min)^2 
   *
   */
  float  getJointLimitGradient(float angle);
  
};

/**
 * class representing a kinematic chain, used for direct and inverse kinematic 
 * \ingroup IK
 *
 * CAUTION : units are *millimeters* and radians 
 *           for the katanas, the joints angle readed on the robot must pass through
 *           the OffsetAngles(G|T)  !
 *
 * Please also note that all the function working with orientation are not well
 * tested, or even known to be not working .. 
 */
class IKModel
{
 public :
 Vector reference;  

 /**
  * creator 
  *
  * \param axes int table of length seg_num  representing each rotation axes direction
  *      0 : x , 1 : y , 2 : z
  * \param segs seg_num Vector representing each segment 
  * \param seg_num numbers of segment in the kinematic chain 
  * \param end_vec Vector representing the end effector 
  */
  IKModel(int * axes,Vector3 * segs, int seg_num,Vector3 end_vec);
  
  /**
   * creator from xml file 
   *
   * read the xml file and create the corresponding model
   * \param filename xml file 
   * see model.xml to see a commented sample of xml description files
   */

  IKModel(const char filename[]);

  /**
   *  direct kinematics computation 
   *
   * return a 3 dimension vector corresponding to the position 
   * of the end effector given the posture described by the angles 
   * Vector
   * \param angles  joints angles values 
   * \return  3d vector : end effector cartesian position
   */
  Vector3 direct(Vector angles);
  
  /**
   * direct kinematics computation 
   *
   * return a 6 d vectors corresponding to position for the first 3 d (mm)
   * and orientation for the last 3 d , orientation is given using quaternions 
   * notation 
   * \param angles joints angles values 
   * \return 6d vector : 
   *      - first 3D position
   *      - last 3D quaternion orientation
   */
  Vector direct_orientation(Vector angles);
  
  /**
   * direct kinematics computation -> homogeneous matrix 
   *
   * return the homogenous matrix corresponding to the transformation 
   * which brings the fixed referential to the end effector referential
   */
  Matrix4 getMatrix4(Vector angles);
  
  /**
   *  compute the jacobian in the given posture
   *
   * \param angles joints angles positions 
   * \return the jacobian : 3 by number of joints matrix 
   */
  Matrix jacobian(Vector angles);

  /**
   * compute the jacobian in the given posture with orientation
   *
   * \param angles joint angles positions
   * \return a 6 by number of joints matrix
   */

  Matrix jacobian_orientation(Vector angles);

  /**
   * inverse kinematics (position only), iterative procedure.
   * 
   * Divide the interval between start and target into small step 
   * and apply inverse_step for each of those.
   * \param start  the initial posture (joints angles )
   * \param target the desired position in cartesian space 
   * \next_angle  is a pointer to the computed posture 
   * \return 1 if posture found, 0 otherwise
   */  
  
  int inverse(Vector start,Vector3 target,Vector * next_angle);

  /**
   * perform one inverse kinematics step  (position only).
   *
   * try to avoid joints limits with the Weighted Least Norm method
   *
   *  \param currentJoints current joints angles position 
   *  \param target desired target cartesian position
   *  \param linSpeedThresh difference between target position and 
   *                        current position will be thresholded to this value 
   *  \param angSpeedThresh difference between returned joints and 
   *                        current joints will be thresholded to this value
   * \return new joint angles values 
   */
  Vector inverse_step(Vector currentJoints,Vector3 target,
		      float linSpeedThresh,float angSpeedThresh);
  
  /**
   * perform one ik step (position only)
   * 
   * use jacobian null space to stay as close as possible from refPosture
   * \param refPosture reference posture joint angles 
   * \param target target cartesian position
   * 
   */
  Vector inverse_step(Vector currentJoints,Vector3 target,
		      float linSpeedThresh,float angSpeedThresh,
		      Vector refPosture);

  /**
   * inverse kinematics (position and orientation )
   * start : is the initial posture (joints angles )
   * target : 6 D vector representing the desired position and orientation  
   * next_angle : is a pointer to the computed posture 
   * return 1 if a posture found, 0 otherwise
   * 
   * DO NOT USE !!! 
   */  
 
  int inverse_orientation(Vector start,Vector target,Vector * next_angles);
 
 /**
   * perform one ik step  (position and orientation)
   * from currentJoints position to target cartesian position and orientation
   * try to avoid joints limits with the Weighted Least Norm method
   *
   * * DO NOT USE !!! 
   */ 
  Vector inverse_step_o(Vector currentJoints,Vector target,
		      float linSpeedThresh,float angSpeedThresh);

 

  /**
   * Write all joint positions to a file in order to use the visualisation tool
   *
   * all joint positions are plotted in the file named "joint_trajectory.txt" 
   * to visualize the corresponding movement, compile the file gl_visu.cpp (make visu) 
   * and call the given program in the folder were the joint_trajectory.txt lies (./visu) 
   */
  void plotPosition();

  /**
   * print the geometry of the model (DEBUG purposes)
   */
  void dump(void);

 protected :
  std::ofstream * f; // joint_trajectory file
  Segment ** segs; // Segments array 
  short seg_num; // number of segments in the model
  Vector3 end_vector; // end effector Vector
  Vector * joint_positions;  // joint positions in cartesian space
  Vector HMatrixToPosition(Matrix4 Homogenous); // convert an Homogenous Matrix to a 3d position vector 
  
};
